/*
 * @Description:
 * @Author: sunm
 * @Github: https://github.com/sunmiaozju
 * @LastEditors: sunm
 * @Date: 2019-03-01 11:26:40
 * @LastEditTime: 2019-03-11 14:23:36
 */
#ifndef LIDAR_EUCLIDEAN_CLUSTER_H
#define LIDAR_EUCLIDEAN_CLUSTER_H

#include <chrono>
#include <cmath>
#include <iostream>

#include <ros/ros.h>

#include <pcl/common/common.h>
#include <pcl/features/don.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/search/kdtree.h>
#include <pcl/search/search.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_ros/point_cloud.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <jsk_recognition_msgs/BoundingBox.h>
#include <jsk_recognition_msgs/BoundingBoxArray.h>

#include <cv.h> 
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <Eigen/Dense>

#include <darknet_ros_msg/BoundingBox.h>
#include <darknet_ros_msg/BoundingBoxes.h>


#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <cmath>
#include <vector>

#include "cluster.h"

namespace LidarDetector {

class PointXYZRT {
public:
    pcl::PointXYZ point;

    float radius;
    float theta;

    size_t radial_div;
    size_t concentric_div;

    size_t original_index;
};
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2, darknet_ros_msg::BoundingBoxes> MySyncPolicy;
typedef std::vector<PointXYZRT> PointCloudXYZRT;

class LidarClusterDetector {
public:
    LidarClusterDetector();

    ~LidarClusterDetector();

private:
 
    struct PicturePoint   //储存左右锥桶的形心
    {

    int x;
    int y;

    };

    struct CentralPoint   //中点坐标
    {

    int x;
    int y;

    };


    struct Detected_Obj     // 存储检测到的障碍物的信息
    {
    jsk_recognition_msgs::BoundingBox bounding_box_;

    pcl::PointXYZ min_point_;
    pcl::PointXYZ max_point_;
    pcl::PointXYZ centroid_;
    };

    ros::NodeHandle nh; 
    ros::NodeHandle private_nh;
    bool processing_now;
    
    ros::Subscriber sub_rawimage;
    ros::Subscriber sub_rawPointCloud;
    
    message_filters::Subscriber<sensor_msgs::Image> image_sub;
    //message_filters::Subscriber<sensor_msgs::CompressedImage> image_sub;
    message_filters::Subscriber<sensor_msgs::PointCloud2> info_sub;
    message_filters::Subscriber<darknet_ros_msg::BoundingBoxes> bboxes_sub;


    message_filters::Synchronizer<MySyncPolicy> sync;

    
    ros::Publisher floor_PointCloud;
    ros::Publisher no_floor_PointCloud;
    ros::Publisher pub_bounding_boxs_;
    ros::Publisher project2ImagePublisher;

    //ros::Publisher pub_clusters;
    ros::Publisher pub_clusters_central;
    //std_msgs::Header point_cloud_header_;

    std::chrono::system_clock::time_point start_time, end_time;
    std_msgs::Header msg_header;
    std_msgs::Header img_header;

    
    double nearDistance;
    double farDistance;
    double leaf_size;
    double height_threshhold;
    double floor_max_height;
    double floor_max_angle;
    double small_scale;
    double large_scale;
    double angle_threshold;
    double radial_divide_angle;
    double concentric_divide_distance;
    double min_local_height_threshold;
    double sensor_height;
    double local_threshold_slope;
    double general_threshold_slope;
    double left_right_dis_threshold;
    
    std::vector<double> seg_distance_, cluster_distance_;
    int MIN_CLUSTER_SIZE; 
    int MAX_CLUSTER_SIZE; 

    //cv::Mat invRt, invTt;
    //bool init_matrix ;
    //cv::Mat CameraExtrinsicMat(4, 4, cv::DataType<double>::type); //cameraExtrinsicMat
    //cv::Mat CameraMat(3, 3, cv::DataType<double>::type);//cameraMat
    //cv::Mat DistCoeff(1, 5, cv::DataType<double>::type);//distCoeff

    int image_w ;
    int image_h ;

    //clusters_ros_msgs::Cluster_points cluster_pointsRseults;
    int x_cluster_central_image;  //聚类中心投影到图上x坐标  project2image function
    int y_cluster_central_image;  //聚类中心投影到图上y坐标 
    Eigen::Matrix4f RT;
    Eigen::Matrix3f camera_param;



    cv::Mat cv_raw_image;  //ros转化的cv格式原始图片
    
    //cv::Mat project_image; //点云投影到图片后的图像



    double cluster_min_points;
    double cluster_max_points;

    std::vector<double> dis_range;
    std::vector<double> seg_distances;
    std::string str_range;
    std::string str_seg_distances;
   
    //std::vector<Detected_Obj> global_obj_list;
    std::vector<cv::Scalar> color_table;
    
    //std::vector<ClusterPtr> clusters;
    std::vector<pcl::PointXYZ> cluster_centroids;

    
    void differenceOfNormalsSegmentation(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud,
        pcl::PointCloud<pcl::PointXYZ>::Ptr& out_cloud);

    void removeFloor(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud,
        pcl::PointCloud<pcl::PointXYZ>::Ptr& out_cloud,
        pcl::PointCloud<pcl::PointXYZ>::Ptr& only_floor_cloud,
        const double& max_height, const double& floor_max_angle);

    void removeFloorRayFiltered(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud,
        pcl::PointCloud<pcl::PointXYZ>::Ptr& out_only_ground_cloud,
        pcl::PointCloud<pcl::PointXYZ>::Ptr& out_no_ground_cloud,
        const double& sensor_height, const double& local_max_slope, const double& general_max_slope);

    void convertXYZ2XYZRT(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud,
        std::vector<PointCloudXYZRT>& out_radial_divided_cloud);

    void mergeClusters();

    void downsampleCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud,
        pcl::PointCloud<pcl::PointXYZ>::Ptr& out_cloud,
        const double& leaf_size);

    void clipCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud,
        pcl::PointCloud<pcl::PointXYZ>::Ptr& out_cloud,
        const double& height, const double& near_dis, const double& fat_dis, const double& left_right_dis);

    void cluster_segment(pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc,
                                    double in_max_cluster_distance, std::vector<Detected_Obj> &obj_list);
    
    void cluster_by_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc, std::vector<Detected_Obj> &obj_list);

    void clusterCpu(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud,
        std::vector<ClusterPtr>& cluster, std::vector<pcl::PointXYZ>& cluster_centroids,const double& max_cluster_dis);

    void clusterGpu();

    void segmentByDistance(const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr);


// *********************************单独回调*************************
    void ImageCallback(const sensor_msgs::Image::ConstPtr& image_msg);
    void getPointCloud_cb(const sensor_msgs::PointCloud2ConstPtr& msg_rawPointCloud);
    void project2image(std::vector<Detected_Obj>& global_obj_list, cv::Mat& raw_image);  

// *********************************单独回调*************************
    
// *********************************融合回调*************************

    void resetMatrix();
    void initMatrix(const cv::Mat& cameraExtrinsicMat);



    void pointcloud2_to_image(std::vector<Detected_Obj>& global_obj_list,const cv::Mat& raw_image,const darknet_ros_msg::BoundingBoxesConstPtr& msg_bboxs);
    void CV_project(std::vector<Detected_Obj>& global_obj_list,cv::Mat& image);

    void datasfusionCallback(const sensor_msgs::Image::ConstPtr& image_msg,const sensor_msgs::PointCloud2ConstPtr& msg_rawPointCloud,const darknet_ros_msg::BoundingBoxesConstPtr& msg_bboxs);

    //void datasfusionCallback(const sensor_msgs::CompressedImage::ConstPtr& image_msg,const sensor_msgs::PointCloud2ConstPtr& msg_rawPointCloud);




    
// *********************************融合回调*************************
    	
    void initROS();

    void pubPointCloud(const ros::Publisher& publisher,const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_pointcloud);
        
        

    void splitString(const std::string& in_string, std::vector<double>& out_array);

//*********************************拟合画函数************************

    /*double sum(vector<double> Vnum, int n);
    double MutilSum(vector<double> Vx, vector<double> Vy, int n);
    double RelatePow(vector<double> Vx, int n, int ex);
    double RelateMutiXY(vector<double> Vx, vector<double> Vy, int n, int ex);
    void EMatrix(vector<double> Vx, vector<double> Vy, int n, int ex, double coefficient[]);
    void CalEquation(int exp, double coefficient[]);
    double F(double c[],int l,int m);
    double Em[6][4];*/

    cv::Mat polyfit(std::vector<cv::Point>& in_point, int n);
    
};

} // namespace LidarDetector

#endif // LIDAR_EUCLIDEAN_CLUSTER_H
